#include "dyn_ode_si.h"

colvec SI::next(colvec previousStep)
{
    // We separate the q and p parts.
    // Compute length of vector:
    int dimension = previousStep.n_rows;

    // Actually divide the vectors.
    colvec positionQ = previousStep.subvec(span(0,dimension/2-1));
    colvec momentumP = previousStep.subvec(span(dimension/2,dimension-1));

    // We now apply the actual algorithm.
    for (int i=0;i<M;i++)
    {
        momentumP += d(i)*initStepsize*system.p(positionQ);
        positionQ += c(i)*initStepsize*system.q(momentumP);
    }

    previousStep.subvec(span(0,dimension/2-1)) = positionQ;
    previousStep.subvec(span(dimension/2,dimension-1)) = momentumP;

    return previousStep;

}
